#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/transform_stamped.hpp"
#include "tf2_ros/static_transform_broadcaster.h"
#include "tf2/LinearMath/Quaternion.h"

#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
class StaticTFBroadcaster:public rclcpp::Node
{
public:
    StaticTFBroadcaster(char *argv[]):Node("static_tf_broadcaster_node")
    {
        RCLCPP_INFO(this->get_logger(),"静态坐标发布节点已创建！");
        stb_=std::make_shared<tf2_ros::StaticTransformBroadcaster>(this);
        this->publisher_transform(argv); 
    }
private:    
    void publisher_transform(char*argv[])
    {
        auto ts=geometry_msgs::msg::TransformStamped();
        ts.header.stamp=this->get_clock()->now();
        ts.header.frame_id=argv[7];
        ts.child_frame_id=argv[8];
        ts.transform.translation.x=atof(argv[1]);
        ts.transform.translation.y=atof(argv[2]);
        ts.transform.translation.z=atof(argv[3]);

        tf2:: Quaternion qtn;
        qtn.setRPY(atof(argv[4]),atof(argv[5]),atof(argv[6]));  
        
        ts.transform.rotation=tf2::toMsg(qtn); //可以一次性转完
        // ts.transform.rotation.x=qtn.x();
        // ts.transform.rotation.x=qtn.y();
        // ts.transform.rotation.x=qtn.z();
        // ts.transform.rotation.x=qtn.w();
        stb_->sendTransform(ts);
    }
private:
    std::shared_ptr<tf2_ros::StaticTransformBroadcaster>stb_;   
};
int main(int argc,char*argv[])
{
    if(argc!=9)
    {
        RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"传入的参数不合法");
        return 1;
    }
    rclcpp::init(argc,argv);
    auto node=std::make_shared<StaticTFBroadcaster>(argv);
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}